#include <PID_v1.h>
#include <MsTimer2.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "EEPROM.h"
#include "Define.h"
#include "Interrupt.h"
#include "E2PRom.h"
#include "SerialControl.h"
#include "MotorDriver.h"

void setup()
{
  delay(50);

  /* TP6612电机控制引脚 */
  pinMode(AN1, OUTPUT);
  pinMode(AN2, OUTPUT);
  pinMode(BN1, OUTPUT);
  pinMode(BN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(STBY, OUTPUT);

  /* LED控制引脚，用于指示电量状态 */
  pinMode(13, OUTPUT);

  /* TB6612使能引脚 */
  digitalWrite(STBY, HIGH);

  /* 上电默认电机为刹车状态 */
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);

  /* 从Arduino芯片内部的EEPROM中加载参数 */
  GetParamers();
  speedcount = sc;
  Offset_Setpoint = osp;

  /* 初始化串口，波特率为115200 */
  Serial.begin(115200);

  /* 初始化I2C总线 */
  Wire.begin();
  delay(100);

  /* 初始化MPU6050 */
  mpu.initialize();
  delay(2);

  /* 启用MPU6050内部的DMP引擎 */
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0)
  {
    mpu.setDMPEnabled(true);

    /* 设置PIN2接收MPU6050的DMP中断 */
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    /* 标记DMP引擎启用成功 */
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }

  /*  */
  Setpoint = Offset_Setpoint;

  /* 设置直立环PID参数 */
  myPID.SetTunings(kp, ki, kd);
  myPID.SetOutputLimits(-255 + dl, 255 - dl);
  myPID.SetSampleTime(5);
  myPID.SetMode(AUTOMATIC);

  /* 设置位置环PID参数，有的人喜欢叫速度环 */
  sPID.SetTunings(sp, si, sd);
  sPID.SetOutputLimits(-10, 70);
  sPID.SetSampleTime(200);
  sPID.SetMode(AUTOMATIC);

  /* 使用PIN3接收电机编码器计数值 */
  attachInterrupt(1, speed, RISING);

  /* 启用一个500ms周期的定时器 */
  /* 用于周期性的检测电池电量 */
  MsTimer2::set(500, flash);
  MsTimer2::start();
}

void loop()
{
  /* 判断MPU6050的DMP引擎是否启动成功 */
  /* 启动失败则loop函数啥都不干了 */
  if (!dmpReady)
    return;

  /* MPU中断没有到来或者FIFO中拿到的数据不够 */
  if (!mpuInterrupt && fifoCount < packetSize)
  {
    return;
  }

  /* 清除中断标记 */
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    /* 异常情况，清空FIFO */
    mpu.resetFIFO();
  }
  else if (mpuIntStatus & 0x02)
  {
    /* 正常情况，判断FIFO中取出的数据够不够 */
    while (fifoCount < packetSize)
    {
      /* 不够的话，再拿一点 */
      fifoCount = mpu.getFIFOCount();
    }
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    /* 获取四元数 */
    mpu.dmpGetQuaternion(&q, fifoBuffer);

    /* 获取重力加速度 */
    mpu.dmpGetGravity(&gravity, &q);

    /* 得到俯仰角、横滚角、偏航角，ypr[3]是一个三维的数组 */
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    /* 计算小车倾角 */
    angle = -ypr[2] * 180 / M_PI;
  }

  /* 第一级PID，位置环，很多人喜欢叫速度环 */
  /* 输入：小车旋转位置 */
  /* 输出：小车目标倾角补偿值 */
  Inputs = speedcount;
  sPID.Compute();

  /* 第二级PID，直立环 */
  /* 输入：小车倾角 */
  /* 输出：电机PWM波 */
  Setpoint = Offset_Setpoint + Outputs;
  Input = angle;
  myPID.Compute();

  /* 如果倾角太大，则直接放弃治疗，电机不转了 */
  if (angle > 60 || angle < -60)
    Output = 0;

  /* PID输出，控制电机转动 */
  motor(Output);

  /* 串口蓝牙模块，用于蓝牙遥控 */
  SerialControl();

  /* 如果蓝牙重新设置了参数，则这个位置将新参数写入 */
  sPID.SetTunings(sp, si, sd);
  myPID.SetTunings(kp, ki, kd);

  /* 发送当前参数信息 */
  SendData();
}
